#include "opencv2/opencv.hpp"
#include <iostream>
#include <vector>

void matchMin(std::vector<cv::DMatch> matches, std::vector<cv::DMatch> &goodMatches)
{
	double minDist = 10000, maxDist = 0;
	for (size_t i = 0; i < matches.size(); ++i)
	{
		double temp = matches[i].distance;
		if (temp < minDist)
		{
			minDist = temp;
		}
		if (temp > maxDist)
		{
			maxDist = temp;
		}
	}
	for (size_t i = 0; i < matches.size(); ++i)
	{
		if (matches[i].distance <= cv::max(2 * minDist, 20.0))
		{
			goodMatches.push_back(matches[i]);
		}
	}
}

// RANSAC算法
void ransac(std::vector<cv::DMatch> matches, std::vector<cv::KeyPoint> queryKeyPoint,
			std::vector<cv::KeyPoint> trainKeyPoint, std::vector<cv::DMatch> &matchesRansac, cv::Mat &H)
{
	// 定义保存匹配点的坐标
	std::vector<cv::Point2f> srcPoints(matches.size()), dstPoints(matches.size());
	// 保存从关键点中提取到的匹配点对坐标
	for (size_t i = 0; i < matches.size(); ++i)
	{
		srcPoints[i] = queryKeyPoint[matches[i].queryIdx].pt;
		dstPoints[i] = trainKeyPoint[matches[i].trainIdx].pt;
	}

	// 匹配点对进行RANSAC过滤
	std::vector<int> inliersMask(srcPoints.size());
	H = cv::findHomography(srcPoints, dstPoints, cv::RANSAC, 5, inliersMask);
	// 手动保留RANSAC过滤后的匹配点对
	for (size_t i = 0; i < inliersMask.size(); ++i)
	{
		if (inliersMask[i])
		{
			matchesRansac.push_back(matches[i]);
		}
	}
}

void orbFeature(cv::Mat &img, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptions)
{
    cv::Ptr<cv::ORB> orb = cv::ORB::create(1000, 1.2f);
    orb->detect(img, keyPoints);
    orb->compute(img, keyPoints, descriptions);
}

double getAngle(cv::Mat H)
{
	//    double x,y;
	//    x=cvmGet(H,0,2);y=cvmGet(H,1,2);
	//缩放的比例
	double Sx,Sy;
	Sx=(double)sqrt( (double)H.at<double>(0,0)*H.at<double>(0,0) + (double) H.at<double>(1,0)*H.at<double>(1,0) );
	Sy=(double)sqrt( (double)H.at<double>(1,0)*H.at<double>(1,0) + (double)H.at<double>(1,1)*H.at<double>(1,1) );
	std::cout<<"缩放系数： "<<Sx<<std::endl;
	//旋转的角度
	double angle = acos(H.at<double>(0,0)/Sx)/M_PI*180;
	std::cout<<"角度： "<<angle<<std::endl;
	return angle;
}

int main(int argc, char *argv[])
{
    cv::Mat img1 = cv::imread("/media/st/Application/Ubuntu/dataset/tj/1019/img/1019/training/004112.jpg");
    cv::Mat img2 = cv::imread("/media/st/Application/Ubuntu/dataset/tj/1019/img/1019/training/004124.jpg");
    if(img1.empty() || img2.empty())
    {
        std::cout << "图片读取错误,请检查是否存在" << std::endl;
        exit(EXIT_FAILURE);
    }

    // 提取ORB特征点
    std::vector<cv::KeyPoint> keyPoints1, keyPoints2;
    cv::Mat description1, description2;
    orbFeature(img1, keyPoints1, description1);
    orbFeature(img2, keyPoints2, description2);

	// 特征点匹配
	std::vector<cv::DMatch> matches, goodMin, goodRansac;
	cv::BFMatcher matcher(cv::NORM_HAMMING);
	matcher.match(description1, description2, matches);

	// 最小汉明距离
	matchMin(matches, goodMin);

	// RANSAC算法筛选
	cv::Mat H;
	ransac(goodMin, keyPoints1, keyPoints2, goodRansac, H);  //相对于第一张图片
	std::cout<<"H "<<H<<std::endl;
	getAngle(H);

	// 绘制匹配结果
	cv::Mat out1, out2, out3;
	cv::drawMatches(img1, keyPoints1, img2, keyPoints2, matches, out1);
	cv::drawMatches(img1, keyPoints1, img2, keyPoints2, goodMin, out2);
	cv::drawMatches(img1, keyPoints1, img2, keyPoints2, goodRansac, out3);
	cv::imshow("未筛选", out1);
	cv::imshow("最小汉明距离筛选", out2);
	cv::imshow("RANSAC筛选", out3);
	cv::waitKey(0);
}